#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;

double position[3]={0,0,0};

void state_cb(const mavros_msgs::State::ConstPtr& msg)
    {
        current_state = *msg;
    }

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
    {
        position[0]=msg->pose.position.x;
        position[1]=msg->pose.position.y;
        position[2]=msg->pose.position.z;
    }

int main(int argc,char **argv)
{
    ros::init(argc, argv,"position");
    ros::NodeHandle nh;

    double hight=0.50;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose",10,pos_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    ros::Rate rate(20.0);

    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;//欲发布的位置信息
    pose.pose.position.x = 0.0;
    pose.pose.position.y = 0.0;
    pose.pose.position.z = 0.0;

    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";//用于请求offboard

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = false;//用于请求shangshuo

    mavros_msgs::SetMode manual_mode;
    manual_mode.request.custom_mode = "MANUAL";//用于请求进入MANUAL模式 //STABILIZED

    // mavros_msgs::SetMode land_set_mode;
    // land_set_mode.request.custom_mode = "AUTO.LAND";//用于执行完任务后的自动降落

    int update=0;
    int a=0;

    ros::Time last_request = ros::Time::now();
    ROS_INFO("Armed");

    while(ros::ok())
    {
        // if( current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(5.0)))
        // {
        //     if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.mode_sent)
        //     {
        //         ROS_INFO("Offboard enabled");
        //     }
        //     last_request = ros::Time::now();
        // } //自动请求offboard

            if(current_state.armed)
            {
                ROS_INFO("Armed");
                if(current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(2.0)))
                {
                    last_request = ros::Time::now();
                    if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.mode_sent){
                    ROS_INFO("Offboard enabled");
                    break;
                    }
                }
            }  
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    
    //------------------------------------------------------------------------------------------

    while (ros::ok())
    {
        if((update==0)&&(abs(position[0])<0.1&&abs(position[1])<0.1&&abs(position[2])<0.1)
            &&(ros::Time::now()-last_request > ros::Duration(1.0)))
        {
            ROS_INFO("wu hu");
            pose.pose.position.x = 0.0;
            pose.pose.position.y = 0.0;
            pose.pose.position.z = hight;
            last_request = ros::Time::now();
        }

        if((update==0)&&(abs(position[0])<0.1&&abs(position[1])<0.1&&abs(position[2]-hight)<0.1)
            &&(ros::Time::now()-last_request > ros::Duration(1.0)))
        {
            update=1;//防止死循环
            last_request = ros::Time::now();
            ROS_INFO("0_0_hight");
        }

        if((update==1)&&(abs(position[0])<0.1&&abs(position[1])<0.1&&abs(position[2]-hight)<0.1)
            &&(ros::Time::now()-last_request > ros::Duration(10.0)))
        {
            ROS_INFO("already position10s");
            pose.pose.position.x = 0.0;
            pose.pose.position.y = 0.0;
            pose.pose.position.z = 0.0;
            last_request = ros::Time::now();
        }

        if((update==1)&&(abs(position[0])<0.1&&abs(position[1])<0.1&&abs(position[2])<0.1))
        {
            break;
        }
 
        local_pos_pub.publish(pose);//一直在while里发送期望位置
        ros::spinOnce();
        rate.sleep();
    }

    while (ros::ok())
    {
        if (current_state.mode != "MANUAL"){
        set_mode_client.call(manual_mode);
        }
        if (current_state.mode == "MANUAL")
        {
            ROS_INFO("manualed");
            arming_client.call(arm_cmd);//请求客户端上锁
            if (arm_cmd.response.success)
            {
                ROS_INFO("locked");
                break;
            }  
        }
    ros::spinOnce();
    rate.sleep();
    }

    return 0;
}
